#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     Lift,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     Arm,           tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C2_1,    Wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#define SERVO_START_POS 0
#define SERVOS_MAX_VALUE 250
#define SERVOS_MIN_VALUE 0

int threshold = 20;
int wristPos =0;

task main()
{
	while(true)
	{
		if(abs(joystick.joy2_y1) > threshold)
		{
			if(joystick.joy2_y1 > 0)
			{
				motor[Lift] = 100;
			}
			else if(joystick.joy2_y1 < 0)
			{
				motor[Lift] = -100;
			}
		}
		else
		{
			motor[Lift] = 0;
		}

		if(abs(joystick.joy2_y2) > threshold)
		{
			if(joystick.joy2_y2 > 0)
			{
				motor[Arm] = 25;
			}
			else if(joystick.joy2_y2 < 0)
			{
				motor[Arm] = -25;
			}
		}
		else
		{
			motor[Arm] = 0;
		}

		{
		if(joystick.joy2_TopHat == 0)
		{
			if (wristPos < SERVOS_MAX_VALUE)
			{
				wristPos = wristPos+3;
			}
		}
		else if (joystick.joy2_TopHat == 4)
		{
			if (wristPos > SERVOS_MIN_VALUE)
			{
				wristPos = wristPos-3;
			}
		}
		servoTarget[Wrist] = wristPos;
		wait10Msec(1);
		}
	}
}
